﻿using System;
using System.Collections.Generic;
using System.Text;

namespace LocalizationTest.Tools
{
  public class Pose2D
  {
    public Vector2 translation;
    public double rotation;

    public Pose2D()
    {
      translation = new Vector2();
      rotation = 0;
    }

    public Pose2D(double x, double y, double rotation)
    {
      this.translation = new Vector2(x, y);
      this.rotation = rotation;
    }

    public Pose2D(Vector2 translation)
    {
      this.translation = translation;
      this.rotation = 0;
    }

    public Pose2D(Vector2 translation, double rotation)
    {
      this.translation = translation;
      this.rotation = rotation;
    }

    public Pose2D(double rotation)
    {
      this.translation = new Vector2();
      this.rotation = rotation;
    }

    public static Vector2 operator *(Pose2D pose, Vector2 point)
    {
      double s = Math.Sin(pose.rotation);
      double c = Math.Cos(pose.rotation);
      return (new Vector2(point.x * c - point.y * s, point.x * s + point.y * c) + pose.translation);
    }

    public static Pose2D operator +(Pose2D a, Vector2 b)
    {
      return new Pose2D(
        a.translation.x + b.Abs() * Math.Cos(a.rotation + b.GetAngle()), 
        a.translation.y + b.Abs() * Math.Sin(a.rotation + b.GetAngle()),
        a.rotation );
    }

    public static Pose2D operator +(Pose2D a, Pose2D b)
    {
      return new Pose2D(
        a * b.translation, MathTools.Normalize(a.rotation + b.rotation) );
//        a.translation.x + b.translation.Abs() * Math.Cos(a.rotation + b.translation.GetAngle()),
//        a.translation.y + b.translation.Abs() * Math.Sin(a.rotation + b.translation.GetAngle()),
//        a.rotation + b.rotation);
    }

    public static Pose2D operator -(Pose2D a, Pose2D b)
    {
      Pose2D p = new Pose2D(-b.rotation);
      return p + (new Pose2D(a.translation-b.translation,a.rotation));
//    return new Pose2D(a.translation - b.translation, a.rotation - b.rotation);
    }

    public Pose2D minusDiff(Pose2D diff)
    {
      double rot = this.rotation-diff.rotation;
      double s = Math.Sin(rot);
      double c = Math.Cos(rot);
      return new Pose2D(
        this.translation.x - c * diff.translation.x + s * diff.translation.y,
        this.translation.y - c * diff.translation.y - s * diff.translation.x, rot);
    }

    public override string ToString()
    {
      return
        translation.x.ToString() + ";" +
        translation.y.ToString() + ";" +
        rotation.ToString();
    }

    public static Pose2D Parse(string str)
    {
      string[] parts = str.Split(';');
      return new Pose2D(
        double.Parse(parts[0]), double.Parse(parts[1]), double.Parse(parts[2]) );
    }
  }
}
